/**
 * @file ROS_control.cpp
 * @author circleup (circleup@foxmail.com)
 * @brief 观光车控制节点，处理串口数据，给下位机发送控制指令
 * @version 2.9
 * @date 2020-07-09
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "ros_serial_driver.h"
#include "ros_can_driver.h"
#include "ros_control_driver.h"
#include "car_pid.h"

#include "ros/ros.h"
#include "ros_car/map_plan.h"

/**
 * @brief 观光车状态数据回调函数
 * 
 * @param msg serial_receive_data 话题消息
 */
void SerialCallback(const ros_car::car_status_frame::ConstPtr& msg);

/**
 * @brief ROS_CAR 观光车控制节点
 * 
 * @param argc 
 * @param argv 
 * @return int 
 */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "ROS_control");

  ROS_INFO("This is ROS_control");

  ros::NodeHandle n;

  ros::Subscriber receive = n.subscribe("serial_receive_data", 1000, SerialCallback);
  ros::Publisher send = n.advertise<ros_car::VCU_frame>("CAN_send_data", 1000);
  ros::ServiceClient client = n.serviceClient<ros_car::map_plan>("ros_map_plan");

  ros::Rate loop_rate(50);

  // BYTE vcu_frame_data_test[8] = {0xF9, 0x00, 0xFF, 0xFF, 0xFC, 0xFF, 0xF0, 0x00};
  BYTE vcu_frame_data_test[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

  ros_car::VCU_frame send_msg;
  send_msg.uID = DEF_CAN_CHANNEL_SELECT;
  send_msg.nSendType = DEF_SEND_TYPE;
  send_msg.bRemoteFlag = 0;
  send_msg.bExternFlag = 0;
  send_msg.nDataLen = 8;
  send_msg.uTimeFlag = 0;

  memcpy(&(send_msg.arryData), &vcu_frame_data_test, 8);
  SetVCUDefaultValue(&send_msg);
  SetVCUPedValue(&send_msg, 0x00);
  SetVCUSTRAngleValue(&send_msg, 0x00);
  SetVCUBreakValue(&send_msg, 0x00);
  SetVCUGearValue(&send_msg, 0x01);

  ros_car::map_plan srv;
  // for(int i = 0; i < 8; ++i)
  // {
  //     ROS_INFO("send_msg.arrdata[%d] is [0x%x]", i, send_msg.arryData[i]);
  // }

  PID_Init(&turn_PID, 0, 5.5, 0, 1);

  PID_Init(&motor_PID, 1.5, 25, 0.1, 0.7);
  PID_SetOutRange(&motor_PID, 50, -20);

  double angle_err = 0;
  double current_VCU_steer_angle_value = 0;
  double current_VCU_ped_value = 0;
  while (ros::ok())
  {
    
    srv.request.lattitude = serial_msg.lattitude;
    srv.request.longitude = serial_msg.longitude;
    // ROS_INFO("req.lattitude is %s, req.longitude is %s", DoubleToString(srv.request.lattitude, 8).c_str(), DoubleToString(srv.request.longitude, 8).c_str());

    if (client.call(srv))
    {
      ROS_INFO("angle: %f,status: %d", srv.response.angle, srv.response.status);
    }
    else
    {
      ROS_ERROR("Failed to call service map_plan");
      return 1;
    }

    if (true == srv.response.status)
    {
      angle_err = serial_msg.heading - srv.response.angle;
      if (fabs(angle_err) > 180)
      {
        if (angle_err > 0)
        {
          angle_err = angle_err - 360;
        }
        else
        {
          angle_err = 360 + angle_err;
        }
      }
      ROS_INFO("angle_err is %f", angle_err);
      current_VCU_steer_angle_value = -(int16_t)Position_PID_Cal(&turn_PID, angle_err);
      ROS_INFO("current_VCU_steer_angle_value is %f", current_VCU_steer_angle_value);
      current_VCU_steer_angle_value -= 60;
      SetVCUSTRAngleValue(&send_msg, current_VCU_steer_angle_value);

      current_VCU_ped_value = Increment_PID_Cal(&motor_PID, serial_msg.velocity);
      ROS_INFO("current_VCU_ped_value is %f", current_VCU_ped_value);
      if(current_VCU_ped_value < 0)
      {
        if (current_VCU_ped_value > -10)
        {
          SetVCUPedValue(&send_msg, 0x00);
          SetVCUBreakValue(&send_msg, 20);
        }
        else
        {
          SetVCUPedValue(&send_msg, 0x00);
          SetVCUBreakValue(&send_msg, 30);
        }
        
        
      }
      else
      {
        SetVCUPedValue(&send_msg, current_VCU_ped_value);
        SetVCUBreakValue(&send_msg, 0x00);
      }
      
      SetVCUGearValue(&send_msg, 0x02);
    }
    else
    {
      SetVCUPedValue(&send_msg, 0x00);
      SetVCUSTRAngleValue(&send_msg, 0x00);
      SetVCUBreakValue(&send_msg, 0x64);
      SetVCUGearValue(&send_msg, 0x01);
    }
    
    send.publish(send_msg);
    
    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}

void SerialCallback(const ros_car::car_status_frame::ConstPtr& msg)
{
  serial_msg.heading = msg->heading;
  serial_msg.lattitude = msg->lattitude;
  serial_msg.longitude = msg->longitude;
  serial_msg.velocity = msg->velocity;
  // ROS_INFO("msg.heading is %f",msg->heading);
  // ROS_INFO("msg.lattitude is %f",msg->lattitude);
  // ROS_INFO("msg.longitude is %f",msg->longitude);
  // ROS_INFO("msg.velocity is %f",msg->velocity);
}
